#include "stdafx.h"
#include "Programs\PObH.h"
#include "Utils\PathUtils\HybridGraph.h"

#include "Utils\Polygon_utils\ExtendedPolygon.h"
#include "Utils\ReferencePoint.h"

#include "Utils\UI_utils\Environment.h"
#include "Utils\UI_utils\TimeManager.h"
#include "Utils\UI_utils\Graphics.h"

#include "Utils\Number_utils\AK_conversions_1.h"
#include "Utils\Polygon_utils\PolygonIO.h"


typedef Rational_kernel						K;
typedef Rational_kernel::FT					NT; //number type
typedef Rational_kernel::Point_2			Point;
typedef CGAL::Polygon_2<K>					Polygon;
typedef std::vector<Polygon>				Polygon_vec;
typedef Extended_polygon<K>					Robot_rep;
typedef Reference_point<K>					ReferencePoint;
typedef std::vector<ReferencePoint>			Path;
typedef CGAL::Polygon_set_2<K>				Polygon_set;




void write_path_robotics(Path const& path,string filename,string robot_id)
{
	std::ofstream pathFile;
	pathFile.open(filename);
	pathFile << "id " << robot_id << endl;
	pathFile << path << endl;
	pathFile.close();
}

void write_path_gui(Path const& path,string filename)
{
  std::ofstream os;
  os.open(filename);
  ReferencePoint p0 = path[0];
  long long size=path.size();
  os << to_string(size) << endl;
  double x = CGAL::to_double(p0.get_location().x());
  double y = CGAL::to_double(p0.get_location().y());
  double a = p0.get_rotation().to_angle();
  os << std::to_string((long double)x);
  os << " " << std::to_string((long double)y); 
  os << " " << std::to_string((long double)a) << endl;
  for (unsigned int i = 1; i < path.size(); i++)
  {
	  ReferencePoint p1 = path[i-1];
	  ReferencePoint p2 = path[i];
	  
	  double x1 = CGAL::to_double(p1.get_location().x());
	  double x2 = CGAL::to_double(p2.get_location().x());
	  double y1 = CGAL::to_double(p1.get_location().y());
	  double y2 = CGAL::to_double(p2.get_location().y());
	  double a1 = p1.get_rotation().to_angle();
	  double a2 = p2.get_rotation().to_angle();
	  long double delta = (long double)smallest_angle(a1,a2);
	  os << std::to_string((long double)x2-x1);
	  os << " " << std::to_string((long double)y2-y1); 
	  os << " " << std::to_string((long double)delta) << endl;
  }
}

void display_scene(int argc, char* argv[])
{
  //typedefs

  //loading files from configuration file
  Time_manager tm;
  tm.write_time_log(std::string("start"));
  
  Environment<> env(argc,argv);
  tm.write_time_log(std::string("set environment"));

  //loading scene from environment
  Polygon_vec&       workspace(env.get_workspace());
  Robot_rep			 my_robot(env.get_robot());	

  //load query
  ReferencePoint q_s (env.get_source_configiration());
  ReferencePoint q_t (env.get_target_configiration());

  global_graphics->draw_polygons<K> (workspace, BLUE);
  my_robot.move_absolute(q_s);
  global_graphics->draw_polygon<K> (my_robot.get_absolute_polygon(), GREEN);
  my_robot.move_absolute(q_t);
  global_graphics->draw_polygon<K> (my_robot.get_absolute_polygon(), RED);

  global_graphics->display();
  return;
}

void single_robot_optimization_graphics(int argc, char* argv[])
{
  // ..\\Input : configuration.txt <robot_id> <num_path>
  // ..\\Input\\shay : configuration_shay.txt <robot_id> <num_path>
  //loading files from configuration file
  Time_manager tm;
  int num_path = atoi(argv[3]);
  std::string robot_id = argv[2];
  tm.write_time_log(std::string("start"));
  
  Environment<> env(argc,argv);
  tm.write_time_log(std::string("set environment"));

  //loading scene from environment
  Polygon_vec		workspace(env.get_workspace());
  Robot_rep			my_robot(env.get_robot());
   
  //load query
  ReferencePoint q_s (env.get_source_configiration());
  ReferencePoint q_t (env.get_target_configiration());

  global_graphics->draw_polygons<K> (workspace, BLUE);
  my_robot.move_absolute(q_s);
  global_graphics->draw_polygon<K> (my_robot.get_absolute_polygon(), GREEN);
  my_robot.move_absolute(q_t);
  global_graphics->draw_polygon<K> (my_robot.get_absolute_polygon(), RED);

  Input_reader reader;
  H_Graph<K> hgraph(workspace,my_robot.get_original_polygon(),q_s,q_t);
  std::vector<std::vector<ReferencePoint>> paths;

  for (int t=0; t < num_path; t++){
	std::vector<ReferencePoint>	path;
	std::stringstream filename;
	filename << "path" << t << ".txt";
	reader.read_path<K>(filename.str(),path);
	global_graphics->draw_path<K>(path);
	//write_path_gui(path,filename.str());
	paths.push_back(path);
  }
  global_graphics->display();

  for (int t=0; t < num_path; t++){
	std::vector<ReferencePoint>	path = paths[t];
	hgraph.add_path(path,true);
  }
  std::vector<ReferencePoint>	opt_path = hgraph.compute_optimal_path();
  write_path_robotics(opt_path,"path_opt.txt",robot_id);
  write_path_gui(opt_path,"path_opt_gui.txt");
  
  global_graphics->draw_path(opt_path, my_robot, true);
  global_graphics->display();
  return;
}


void single_robot_optimization(int argc, char* argv[])
{
  // . : ..\Input\\Maze\\MMS\\ Robot_easy.txt Workspace.txt <robot_id> <num_path>
  // . : ..\Input\\shay\\ Robot.txt Workspace.txt <robot_id> <num_path>
  Input_reader reader;
  std::string dir_name = argv[1];
  std::string robot_file_name = dir_name+argv[2];
  std::string workspace_file_name = dir_name+argv[3];
  std::string robot_id = argv[4];
  int num_path = atoi(argv[5]);
  
  //read robot & workspace
  Robot_rep			my_robot;
  Polygon			tmp_robot;
  Polygon_vec	     workspace;
  reader.read_polygons<K> (workspace_file_name,workspace);
  reader.read_polygon<K>(robot_file_name, tmp_robot);
  my_robot = Robot_rep (tmp_robot);
  
  std::vector<std::vector<ReferencePoint>> paths;
  for (int t=0; t < num_path; t++){
	std::vector<ReferencePoint>	path;
	std::stringstream filename;
	filename << dir_name << "path" << t << ".txt";
	reader.read_path<K>(filename.str(),path);
	paths.push_back(path);
  }
  H_Graph<K> hgraph(workspace,my_robot.get_original_polygon(),paths[0]);
  for (int t=0; t < num_path; t++){
	std::vector<ReferencePoint>	path = paths[t];
	hgraph.add_path(path);
  }
  std::vector<ReferencePoint>	opt_path = hgraph.compute_optimal_path();
  write_path_robotics(opt_path,dir_name+"path_opt.txt",robot_id);
  write_path_gui(opt_path,dir_name+"path_opt_gui.txt");
  return;
}





